﻿//---------------------------------------------------------------------------
#pragma hdrstop

#include "Stdafx.h"
#include "PSO_Class.h"

//---------------------------------------------------------------------------

#pragma package(smart_init)
int PointNum = 6;
int ParticalNum = 30;

//---------------------------------------------------------------------------
void Swarm_Store::VirtualTail2Pos(int ParIndex)  //-1為全部 -2為全部的Best
{
	vector_store Vec = calculate_vector(End[0], End[1]);

	if(ParIndex == -2)
	{
		BestPos[ PointNum-1 ].x = End[0].x + BestVirtualTail * Vec.x;
		BestPos[ PointNum-1 ].y = End[0].y + BestVirtualTail * Vec.y;
	}
	else if(ParIndex == -1)
	{
		for (int i = 0; i < ParticalNum; i++)
		{
			Partical[i].Pos[ PointNum - 1 ].x = End[0].x + Partical[i].VirtualTail * Vec.x;
			Partical[i].Pos[ PointNum - 1 ].y = End[0].y + Partical[i].VirtualTail * Vec.y;

			Partical[i].BestPos[ PointNum - 1 ].x = End[0].x + Partical[i].BestVirtualTail * Vec.x;
			Partical[i].BestPos[ PointNum - 1 ].y = End[0].y + Partical[i].BestVirtualTail * Vec.y;
		}
	}
	else
	{
		Partical[ ParIndex ].Pos[ PointNum - 1 ].x = End[0].x + Partical[ ParIndex ].VirtualTail * Vec.x;
		Partical[ ParIndex ].Pos[ PointNum - 1 ].y = End[0].y + Partical[ ParIndex ].VirtualTail * Vec.y;

		Partical[ ParIndex ].BestPos[ PointNum - 1 ].x = End[0].x + Partical[ ParIndex ].BestVirtualTail * Vec.x;
		Partical[ ParIndex ].BestPos[ PointNum - 1 ].y = End[0].y + Partical[ ParIndex ].BestVirtualTail * Vec.y;
	}
}

void Swarm_Store::Pos2VirtualTail(int ParIndex)   //-1為全部 -2為全部的Best
{
    if(ParIndex == -2)
	{
		BestVirtualTail = calculate_distance(End[0], BestPos[ PointNum-1 ]);
	}
	else if(ParIndex == -1)
	{
		for (int i = 0; i < ParticalNum; i++)
		{
			Partical[i].VirtualTail = calculate_distance(End[0], Partical[i].Pos[ PointNum - 1 ]);

			Partical[i].BestVirtualTail = calculate_distance(End[0], Partical[i].BestPos[ PointNum - 1 ]);
		}
	}
	else
	{
		Partical[ ParIndex ].VirtualTail = calculate_distance(End[0], Partical[ ParIndex ].Pos[ PointNum - 1 ]);

		Partical[ ParIndex ].BestVirtualTail = calculate_distance(End[0], Partical[ ParIndex ].BestPos[ PointNum - 1 ]);
	}
}

void Swarm_Store::AllInitial()
{
    BestFitness = -100;
	float EndDis = calculate_distance(End[0], End[1]);
	vector_store Vec = calculate_vector(End[0], End[1]);
	float Shift, Dis;
	position_store TempPos;
	vector_store TempVec;

	for (int i = 0; i < ParticalNum; i++)
	{
        //起點
		Partical[i].Pos[0] = Start;

        //終點
		Shift = RandRange(0, EndDis);
		Partical[i].Pos[ PointNum - 1 ].x = End[0].x + Shift*Vec.x;
		Partical[i].Pos[ PointNum - 1 ].y = End[0].y + Shift*Vec.y;

        //計算距離
		Dis = calculate_distance( Partical[i].Pos[0], Partical[i].Pos[PointNum-1] );
		//分成五段
		Dis /= ( (float)(PointNum-1) );

		//紀錄頭到尾的向量
		TempVec = calculate_vector( Partical[i].Pos[0], Partical[i].Pos[PointNum-1] );

        Partical[i].Fitness = -100;
		Partical[i].BestFitness = -100;
		//其他點
		for (int j = 1; j < PointNum-1; j++)
		{
			//先讓他排成一直線
			Partical[i].Pos[j] = position_shift(Partical[i].Pos[0], TempVec, 0, j*Dis);

			//在讓他隨意移動
			if(i > ParticalNum/2)
			{
				Partical[i].Pos[j] = position_shift(Partical[i].Pos[j], TempVec, RandRange(-Dis,Dis), RandRange(-Dis/2,Dis/2));
			}

//			Partical[i].Pos[j].x = RandRange(0, 640);
//			Partical[i].Pos[j].y = RandRange(0, 480);
//			Partical[i].BestPos[j] = Partical[i].Pos[j];
		}

		for (int j = 0; j < PointNum; j++)
		{
			Partical[i].BestPos[j] = Partical[i].Pos[j];

			Partical[i].Vec[j].x = 0;
			Partical[i].Vec[j].y = 0;
		}
	}

	for (int j = 0; j < PointNum; j++)
	{
		BestPos[j] = Partical[0].Pos[j];
	}
}

void Swarm_Store::CalculateAllFit()  //計算適應函數
{
	float Dis, Length, Obstacle;
	vector_store Vec;
	position_store TempPos;

    for (int i = 0; i < ParticalNum; i++)
	{
		Length = 1;
		Obstacle = 1;
		for (int j = 0; j < PointNum-1; j++)
		{
			Dis = calculate_distance( Partical[i].Pos[j], Partical[i].Pos[j+1]);
			Length += Dis;

			Vec = calculate_vector(Partical[i].Pos[j], Partical[i].Pos[j+1]);

			for (int k = 0; k < Dis; k+=dt)
			{
				TempPos.x = Partical[i].Pos[j].x + k*Vec.x;
				TempPos.y = Partical[i].Pos[j].y + k*Vec.y;

				for (int x_shift = -detect_width; x_shift < detect_width; x_shift++)
				{
					if( ImMap[(int)(TempPos.y)][(int)(TempPos.x+x_shift)] )
					{
						Obstacle ++;
					}
				}
			}
		}
		//ShowMessage(Obstacle);
		Partical[i].Fitness = Ko/Obstacle + Kl/Length;
		if(Obstacle > 1000)
		{
			Partical[i].Fitness = -100;
		}
	}
}

void Swarm_Store::StorePGBest()
{
	for (int i = 0; i < ParticalNum; i++)
	{
		if( Partical[i].Fitness > Partical[i].BestFitness)
		{
			Partical[i].BestFitness = Partical[i].Fitness;
			for (int j = 0; j < PointNum; j++)
			{
				Partical[i].BestPos[j] = Partical[i].Pos[j];
			}
		}

        if( Partical[i].Fitness > BestFitness)
		{
			BestFitness = Partical[i].Fitness;
			for (int j = 0; j < PointNum; j++)
			{
				BestPos[j] = Partical[i].Pos[j];
			}
        }
	}
}

void Swarm_Store::Move()
{
	float Wp, Wg;  //移動的權重植
	Wp = 0.5;
	Wg = 0.5;

	line_parameter_store line = get_line_parameter(End[0], End[1]);
    float Dis1, Dis2;

	for (int i = 0; i < ParticalNum; i++)
	{
		for (int j = 1; j < PointNum-1; j++)
		{
			//頭不能動，從1開始
			Partical[i].Vec[j].x = 0.5*Partical[i].Vec[j].x + ( Wp*2*RandRange(0,1)*( Partical[i].BestPos[j].x - Partical[i].Pos[j].x ) +
									                            Wg*2*RandRange(0,1)*( BestPos[j].x - Partical[i].Pos[j].x ) );

			Partical[i].Vec[j].y = 0.5*Partical[i].Vec[j].x + ( Wp*2*RandRange(0,1)*( Partical[i].BestPos[j].y - Partical[i].Pos[j].y ) +
																Wg*2*RandRange(0,1)*( BestPos[j].y - Partical[i].Pos[j].y ) );
			//開始移動
			Partical[i].Pos[j].x += Partical[i].Vec[j].x;
			Partical[i].Pos[j].y += Partical[i].Vec[j].y;

			Partical[i].Pos[j].x = Partical[i].Pos[j].x > 639 ? 639 : Partical[i].Pos[j].x;
			Partical[i].Pos[j].y = Partical[i].Pos[j].y > 479 ? 479 : Partical[i].Pos[j].y;
		}

		//先計算虛擬點
		Pos2VirtualTail( i );
		Pos2VirtualTail( -2 );
		//由於只需要一維，算X就好
		Partical[i].Vec[ PointNum-1 ].x = Wp*2*RandRange(0,1)*( Partical[i].BestVirtualTail - Partical[i].VirtualTail ) +
										  Wg*2*RandRange(0,1)*( BestVirtualTail - Partical[i].VirtualTail );

		Partical[i].VirtualTail += Partical[i].Vec[ PointNum-1 ].x;
		//把虛擬點映射回來
		VirtualTail2Pos( i );
		VirtualTail2Pos( -2 );


//        //尾巴有限定範圍，先算投影
//		Partical[i].Pos[PointNum-1] = get_projection_point(line, Partical[i].Pos[PointNum-1]);
//		//檢查有沒有超過
//		Dis1 = calculate_distance(Partical[i].Pos[PointNum-1], End[0]);
//		Dis2 = calculate_distance(Partical[i].Pos[PointNum-1], End[1]);
//
//		if( Dis1 + Dis2 > calculate_distance(End[0], End[1]) )
//		{
//			if(Dis1 < Dis2)
//			{
//				Partical[i].Pos[PointNum-1] = End[0];
//			}
//			else
//			{
//				Partical[i].Pos[PointNum-1] = End[1];
//			}
//		}
	//ShowMessage(Partical[i].VirtualTail);
	}
}

void Swarm_Store::Mutation()
{
	float Dis, Dis1, Dis2, Shift;
	float EndDis = calculate_distance(End[0], End[1]);
	vector_store Vec = calculate_vector(End[0], End[1]);
	vector_store TempVec;
	bool ExcuteMutation = false;
	line_parameter_store line = get_line_parameter(End[0], End[1]);

	for (int i = 0; i < ParticalNum; i++)
	{
        //計算距離
		Dis = calculate_distance( Partical[i].Pos[0], Partical[i].Pos[PointNum-1] );
		//分成五段
		Dis /= ( (float)(PointNum-1) );

		//紀錄頭到尾的向量
		TempVec = calculate_vector( Partical[i].Pos[0], Partical[i].Pos[PointNum-1] );

		ExcuteMutation = RandRange(0,100) < MutationRate;
		if( ExcuteMutation )
		{
			for (int j = 1; j < PointNum-1; j++)
			{
				Partical[i].Pos[j] = position_shift(Partical[i].Pos[j], TempVec, RandRange(-Dis,Dis), RandRange(-Dis,Dis));
			}
		}
		if(ExcuteMutation)
		{
			Partical[i].VirtualTail = RandRange( 0, calculate_distance(End[0], End[1]) );
			//把虛擬點映射回來
			VirtualTail2Pos( i );
			VirtualTail2Pos( -2 );

//			//尾巴有限定範圍，先算投影
//			Partical[i].Pos[PointNum-1] = get_projection_point(line, Partical[i].Pos[PointNum-1]);
//			//檢查有沒有超過
//			Dis1 = calculate_distance(Partical[i].Pos[PointNum-1], End[0]);
//			Dis2 = calculate_distance(Partical[i].Pos[PointNum-1], End[1]);
//
//			if( Dis1 + Dis2 > calculate_distance(End[0], End[1]) )
//			{
//				if(Dis1 < Dis2)
//				{
//					Partical[i].Pos[PointNum-1] = End[0];
//				}
//				else
//				{
//					Partical[i].Pos[PointNum-1] = End[1];
//				}
//			}
		}
	}
}

void Swarm_Store::Process()
{
	for (int i = 0; i < MaxEpoch; i++)
	{
		CalculateAllFit();
		StorePGBest();
		Move();
		Mutation();
		paint_All();
		Delay(20);
	}
}

//---------------------------------------------------------------------------
float RandRange(int min, int max)
{
	float result;
	result = ( (float)(max - min) ) * ( (float)rand() ) / ( (float)RAND_MAX ) + (float)min;
	return result;
}

float Float_Abs(float input)
{
	if( input < 0 )
	{
		return -input;
	}
	else
	{
    	return input;
	}
}

float Gaussian(float X, float deviation)  //輸入X與deviation的高斯函數
{
	if(X > 3*deviation)  //如果太大，直接給0
	{
		return 0;
	}
	else if(X < -3*deviation)  //如果太小，直接給0
	{
		return 0;
	}
	else
	{
		return exp( -(X*X) / (float)(2*deviation*deviation) );
	}
}

position_store xy2position(float x, float y)
{
	position_store position;
	position.x = x;
	position.y = y;
	return position;
}

float calculate_distance(position_store position1, position_store position2)
{
	//hypot(x,y)	(x平方+y平方)的根值
	if(
		(position2.x == position1.x)
		&&
		(position2.y == position1.y)    //如果兩點是同一點
	  )
	{
		return 0.0;
	}
	else
	{
		return (float)hypot( (double)(position1.x-position2.x), (double)(position1.y-position2.y) );
	}
}

vector_store calculate_vector(position_store position1, position_store position2)
{
	//注意：此計算方式為後面(position2)減前面(position1)
	//亦即vector=(position1)--->(position2)
	vector_store output_vector;
	float distance;

	if(
		(position2.x == position1.x)
		&&
		(position2.y == position1.y)    //如果兩點是同一點
	  )
	{
		//向量設為0
		output_vector.x = 0;
		output_vector.y = 0;
	}
	else   //如果不是同一點
	{
		distance = calculate_distance(position1, position2);   //計算兩點之間的距離
		output_vector.x = (float)( (float)(position2.x-position1.x)/(float)distance );
		output_vector.y = (float)( (float)(position2.y-position1.y)/(float)distance );
	}

	return output_vector;
}

line_parameter_store get_line_parameter(position_store start, position_store end)  //計算方程式的參數
{
        line_parameter_store line_parameter;  //回傳出去的參數

        //ax+by=c

        //開始計算參數
        line_parameter.a = start.y - end.y;
        line_parameter.b = end.x - start.x;
        line_parameter.c = end.x*start.y - start.x*end.y;

        return line_parameter;
}

float parameter_point_distance(line_parameter_store standard_line_parameter, position_store point)   //利用方程式參數跟一個點來計算直線跟點的距離
{
        float distance;
        distance = abs( standard_line_parameter.a*point.x + standard_line_parameter.b*point.y - standard_line_parameter.c )
                   /
                   hypot( standard_line_parameter.a, standard_line_parameter.b );

        return distance;
}

intersection_store get_intersection_point(line_parameter_store line_parameter1, line_parameter_store line_parameter2)  //利用兩條線的方程式參數來取得其焦點
{
        intersection_store intersection;  //交點的宣告
		float Delta, Delta_X, Delta_Y;

        //先判斷兩條線是否為平行
        Delta = ( line_parameter1.a * line_parameter2.b ) - ( line_parameter1.b * line_parameter2.a );

        if( Delta==0 )  //如果兩條線平行
        {
                //回傳(0,0)
				intersection.parallel = true;

				intersection.point.x = 0.0;
				intersection.point.y = 0.0;
        }
        else   //如果沒有平行
        {
                intersection.parallel = false;

                Delta_X = ( line_parameter1.c * line_parameter2.b ) - ( line_parameter1.b * line_parameter2.c );
				Delta_Y = ( line_parameter1.a * line_parameter2.c ) - ( line_parameter1.c * line_parameter2.a );

				intersection.point.x =  Delta_X / Delta;
				intersection.point.y =  Delta_Y / Delta;
		}

		return intersection;
}

position_store get_projection_point(line_parameter_store line, position_store pos)   //輸入點跟線，回傳點在線上的投影點
{
	position_store result;
	line_parameter_store Vline;  //跟line垂直但通過pos的線

	Vline.a = line.b;
	Vline.b = -line.a;
	Vline.c = Vline.a*pos.x + Vline.b*pos.y;

	//計算兩調線交點
	intersection_store inter = get_intersection_point(line, Vline);

	if( inter.parallel )
	{
		result.x = -1000;
        result.y = -1000;
	}
	else
	{
		result = inter.point;
	}

	return result;
}

position_store position_shift(position_store position, vector_store vector, float right, float forward)    //position危機點vector為方向，往右位移right，往前位移forward的點
{
	position_store result;

	vector_store right_vector;   //紀錄往右的vector
	right_vector.x = -vector.y;
	right_vector.y = vector.x;

	//往前方位移
	result.x = position.x + vector.x * forward;
	result.y = position.y + vector.y * forward;
	//往右邊位移
	result.x = result.x + right_vector.x * right;
	result.y = result.y + right_vector.y * right;

	return result;
}
